// 以当前位置做恒速沿 x 方向运动，x >= 0.1 时停止
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <eigen3/Eigen/Dense>
#include <moveit_servo/servo_parameters.h>
#include <moveit_servo/servo.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>

using namespace std::chrono_literals;

static const rclcpp::Logger LOGGER = rclcpp::get_logger("force_servo_control_node");

class ForceServoControlNode : public rclcpp::Node
{
public:
  ForceServoControlNode() : Node("force_servo_control_node"), count_(0), stopped_(false)
  {
    tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

    twist_cmd_pub_ = create_publisher<geometry_msgs::msg::TwistStamped>(
        "force_servo_control_node/delta_twist_cmds", 10);
  }

  void initialize()
  {
    setupPlanningSceneMonitor();
    setupServo();
    getCurrentPositionAsReference();

    timer_ = create_wall_timer(
        std::chrono::milliseconds(40),
        std::bind(&ForceServoControlNode::control_loop, this));

    RCLCPP_INFO(this->get_logger(), "恒速伺服节点已初始化（沿 base_link.x = 0.02 m/s，阈值 0.1 m）");
  }

private:
  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_cmd_pub_;
  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
  std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> planning_scene_monitor_;
  std::unique_ptr<moveit_servo::Servo> servo_;
  rclcpp::TimerBase::SharedPtr timer_;

  Eigen::Vector3d position_reference_{Eigen::Vector3d::Zero()};
  size_t count_;
  bool stopped_;  // 到达阈值后标记为 true

  void getCurrentPositionAsReference()
  {
    geometry_msgs::msg::TransformStamped transform;
    try
    {
      transform = tf_buffer_->lookupTransform("base_link", "end", tf2::TimePointZero);
      position_reference_.x() = transform.transform.translation.x;
      position_reference_.y() = transform.transform.translation.y;
      position_reference_.z() = transform.transform.translation.z;

      RCLCPP_INFO(this->get_logger(), "参考位置: x=%.3f, y=%.3f, z=%.3f",
                  position_reference_.x(), position_reference_.y(), position_reference_.z());
    }
    catch (tf2::TransformException &ex)
    {
      RCLCPP_WARN(this->get_logger(), "无法获取TF变换: %s", ex.what());
    }
  }

  void control_loop()
  {
    double current_x = 0.0;
    try
    {
      auto transform = tf_buffer_->lookupTransform("base_link", "end", tf2::TimePointZero);
      current_x = transform.transform.translation.x;
    }
    catch (tf2::TransformException &ex)
    {
      RCLCPP_WARN(this->get_logger(), "无法获取末端位置: %s", ex.what());
      return;
    }

    // 判断是否超过阈值
    if (!stopped_ && current_x >= 0.1)
    {
      stopped_ = true;
      RCLCPP_INFO(this->get_logger(), "末端 x=%.3f >= 0.1，停止运动", current_x);
    }

    auto msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
    msg->header.stamp = this->now();
    msg->header.frame_id = "base_link";

    if (stopped_)
    {
      msg->twist.linear.x = 0.0;
    }
    else
    {
      msg->twist.linear.x = 0.04; // 固定速度（m/s）
    }
    msg->twist.linear.y = 0.0;
    msg->twist.linear.z = 0.0;
    msg->twist.angular.x = 0.0;
    msg->twist.angular.y = 0.0;
    msg->twist.angular.z = 0.0;

    twist_cmd_pub_->publish(std::move(msg));
    count_++;
  }

  void setupPlanningSceneMonitor()
  {
    planning_scene_monitor_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
        shared_from_this(), "robot_description", tf_buffer_, "planning_scene_monitor");

    if (planning_scene_monitor_->getPlanningScene())
    {
      planning_scene_monitor_->startStateMonitor("/joint_states");
      planning_scene_monitor_->setPlanningScenePublishingFrequency(25);
      planning_scene_monitor_->startPublishingPlanningScene(
          planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
          "/moveit_servo/publish_planning_scene");
      planning_scene_monitor_->startSceneMonitor();
      planning_scene_monitor_->providePlanningSceneService();
    }
    else
    {
      RCLCPP_ERROR(LOGGER, "规划场景配置失败");
      throw std::runtime_error("Planning scene setup failed");
    }
  }

  void setupServo()
  {
    auto servo_parameters = moveit_servo::ServoParameters::makeServoParameters(shared_from_this());
    if (!servo_parameters)
    {
      RCLCPP_FATAL(LOGGER, "无法加载伺服参数");
      throw std::runtime_error("Failed to load servo parameters");
    }

    servo_ = std::make_unique<moveit_servo::Servo>(shared_from_this(), servo_parameters, planning_scene_monitor_);
    servo_->start();
  }
};

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<ForceServoControlNode>();
  node->initialize();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}
